#ifndef __CAN_SIMPLE_HPP_
#define __CAN_SIMPLE_HPP_

#include "interface_can.hpp"

class CANSimple {
   public:
    enum {
        MSG_CO_NMT_CTRL = 0x000,       //0 CANOpen NMT Message REC
        MSG_ODRIVE_HEARTBEAT,          //1
        MSG_ODRIVE_ESTOP,              //2
        MSG_GET_MOTOR_ERROR,           //3 Errors
        MSG_GET_ENCODER_ERROR,         //4
        MSG_GET_SENSORLESS_ERROR,      //5
        MSG_SET_AXIS_NODE_ID,          //6
        MSG_SET_AXIS_REQUESTED_STATE,  //7
        MSG_SET_AXIS_STARTUP_CONFIG,   //8
        MSG_GET_ENCODER_ESTIMATES,     //9
        MSG_GET_ENCODER_COUNT,         //10
        MSG_SET_CONTROLLER_MODES,      //11
        MSG_SET_INPUT_POS,             //12
        MSG_SET_INPUT_VEL,             //13
        MSG_SET_INPUT_TORQUE,          //14
        MSG_SET_VEL_LIMIT,             //15
        MSG_START_ANTICOGGING,         //16
        MSG_SET_TRAJ_VEL_LIMIT,        //17
        MSG_SET_TRAJ_ACCEL_LIMITS,     //18
        MSG_SET_TRAJ_INERTIA,          //19
        MSG_GET_IQ,                    //20
        MSG_GET_SENSORLESS_ESTIMATES,  //21
        MSG_RESET_ODRIVE,              //22
        MSG_GET_VBUS_VOLTAGE,          //23
        MSG_CLEAR_ERRORS,              //24  38
        MSG_DEBUG,                     //25  39
        MSG_SET_CURRENT_STATE,         //26  3A
        MSG_SET_TEST_STATE,            //27  3B
        MSG_GET_TWO_ENCODER_ESTIMATES, //28  3C
        MSG_GET_CURRENT_MEAS_PHB,      //29  3D
        MSG_GET_TWO_ENCODER_VEL,       //30  3E  
        MSG_GET_ERROR_INFO,            //31  3F 

		MSG_ODRIVE_ENCODER_COUNT,       //32 40
        
        MSG_CO_HEARTBEAT_CMD = 0x700,  //26 CANOpen NMT Heartbeat  SEND
    };

    static void handle_can_message(can_Message_t& msg);
    static void send_heartbeat(Axis* axis);
	static void active_upload_encoder_count(Axis* axis,uint32_t times);  //?��?����?��????��
	static void active_upload_error(Axis* axis);  //?��?����?��?error
	
   private:
    static void nmt_callback(Axis* axis, can_Message_t& msg);
    static void estop_callback(Axis* axis, can_Message_t& msg);
    static void get_motor_error_callback(Axis* axis, can_Message_t& msg);
    static void get_encoder_error_callback(Axis* axis, can_Message_t& msg);
    static void get_controller_error_callback(Axis* axis, can_Message_t& msg);
    static void get_sensorless_error_callback(Axis* axis, can_Message_t& msg);
    static void set_axis_nodeid_callback(Axis* axis, can_Message_t& msg);
    static void set_axis_requested_state_callback(Axis* axis, can_Message_t& msg);
    static void set_axis_startup_config_callback(Axis* axis, can_Message_t& msg);
    static void get_encoder_estimates_callback(Axis* axis, can_Message_t& msg);
    static void get_encoder_count_callback(Axis* axis, can_Message_t& msg);
    static void set_input_pos_callback(Axis* axis, can_Message_t& msg);
    static void set_input_torque_callback(Axis* axis, can_Message_t& msg);
    static void set_controller_modes_callback(Axis* axis, can_Message_t& msg);
    static void set_vel_limit_callback(Axis* axis, can_Message_t& msg);
    static void start_anticogging_callback(Axis* axis, can_Message_t& msg);
    static void set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg);
    static void set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg);
    static void set_traj_inertia_callback(Axis* axis, can_Message_t& msg);
    static void get_iq_callback(Axis* axis, can_Message_t& msg);
    static void get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg);
    static void get_vbus_voltage_callback(Axis* axis, can_Message_t& msg);
    static void clear_errors_callback(Axis* axis, can_Message_t& msg);
    static void set_axis_current_state_callback(Axis* axis, can_Message_t& msg);

	
    static void send_debug(Axis* axis, can_Message_t& msg);	
	static void set_input_vel_callback(Axis* axis, can_Message_t& msg);
	static void get_two_encoder_estimates_callback(Axis* axis, can_Message_t& msg);	
    static void get_two_encoder_shadow_count_20ms_callback(Axis* axis, can_Message_t& msg);
	static void get_drive_error_callback(Axis* axis, can_Message_t& msg);
	
    // Utility functions
    static uint32_t get_node_id(uint32_t msgID);
    static uint8_t get_cmd_id(uint32_t msgID);
    // Fetch a specific signal from the message
    // This functional way of handling the messages is neat and is much cleaner from
    // a data security point of view, but it will require some tweaking
    //
    // const std::map<uint32_t, std::function<void(can_Message_t&)>> callback_map = {
    //     {0x000, std::bind(&CANSimple::heartbeat_callback, this, _1)}
    // };
};





#endif